#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
#from position.msg import Point,target
from sensor_msgs.msg import Image
from position_msg.msg import target,Point
from darknet_ros_msgs.msg import BoundingBoxes
from std_msgs.msg import String
from cv_bridge import CvBridge
import cv2
import numpy as np

bridge = CvBridge()

# 定义发布器
coordinates_pub = rospy.Publisher("/object_3d_coordinates", target, queue_size=10)
coordinates_person = rospy.Publisher('/bottle_coordinates', target, queue_size=10)
coordinates_pub_blue = rospy.Publisher('/blue_coordinates', target, queue_size=10)


global depth_image
voice_word="cup"
#depth_image = cv2.resize(depth_image, (1280, 720), interpolation=cv2.INTER_NEAREST)

global target_name
target_name=""
def target_name_callback(name):
	global target_name
	target_name=name.data

# 回调函数，接收深度信息
def depth_callback(data):
    	# 将深度图像转换为OpenCV格式
	global depth_image
	depth_image = bridge.imgmsg_to_cv2(data)
	# 调整图像大小到1280x720
	depth_image = cv2.resize(depth_image, (1280, 720), interpolation=cv2.INTER_NEAREST)

def voice_callback(data):
	# 将深度图像转换为OpenCV格式
	global voice_word
	voice_word = data.data
	print("识别物品为：",voice_word)

# 回调函数，接收bounding_boxes信息
def bounding_boxes_callback(data):
	global target_name
	global start
	global voice_word
	for box in data.bounding_boxes:
		global depth_image
		depth = get_depth(depth_image, box)  # 自定义函数，根据x和y获取深度值

        # 构建三维坐标消息
		pos = target()
		pos.point.x = box.xmax/2+box.xmin/2# 计算得到的物体的x坐标
		pos.point.y = box.ymax/2+box.ymin/2# 计算得到的物体的y坐标
		pos.point.z = depth# 计算得到的物体的z坐标
		pos.Class=box.Class

        #与目标物体匹配，才输出
		if pos.Class == voice_word:
        #if 1:
            # 设置坐标的参考框架和时间戳
			pos.header.stamp = rospy.Time.now()
			pos.header.frame_id = "camera_rgb_optical_frame"
            

            # 发布三维坐标消息
			if pos.point.z != 0:
				coordinates_person.publish(pos)
                # 打印坐标信息
				print("bottle Target Coordinates: x={0}, y={1}, z={2}".format(pos.point.x*0.001, pos.point.y*0.001, pos.point.z*0.001))
		else:
            # 设置坐标的参考框架和时间戳
			pos.header.stamp = rospy.Time.now()
			pos.header.frame_id = "camera_rgb_optical_frame"

            # 发布三维坐标消息
			coordinates_pub_blue.publish(pos)





def isNaN(n):
	return n!= n
def get_depth(depth_image, box):
	y = int(box.ymax/2 + box.ymin/2)  # 将y坐标转换为整数
	x = int(box.xmax/2 + box.xmin/2)  # 将x坐标转换为整数
	# depth_image = cv2.resize(depth_image, (1280, 720), interpolation=cv2.INTER_NEAREST)
	depth_image = cv2.resize(depth_image, (640, 480), interpolation=cv2.INTER_NEAREST)
	depth_value = depth_image[y, x]
	if isNaN(depth_value):
		return -1
	return depth_value


if __name__=="__main__":
	
# 初始化ROS节点
	rospy.init_node("position_test")
# 识别目标名称
	rospy.Subscriber("/target_name", String, target_name_callback)
# 订阅深度信息
	rospy.Subscriber("/camera/depth/image_rect_raw", Image, depth_callback)
# 订阅bounding_boxes信息
	rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, bounding_boxes_callback)

	rospy.Subscriber("/voice_back", String, voice_callback)
# 开始运行
	rospy.spin()

